#ifndef DWA_H
#define DWA_H

#include <geometry_msgs/Twist.h>
#include <control_msgs/JointControllerState.h>
#include <ackermann_msgs/AckermannDriveStamped.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Int8.h>
#include "distance.hpp"
#include "transform.h"

struct ControlValue{
    double speed;
    double angle;
};

class Dwa{
public:
    Dwa(ros::NodeHandle& n)
    {
        nh = n;
        cal_distance_ptr_.reset(new CalDistance(nh));
        std::string odom_topic,front_topic,rear_topic,control_topic,limit_speed_topic;
        nh.getParam("odom_topic",odom_topic);
        nh.getParam("front_wheel_topic",front_topic);
        nh.getParam("rear_wheel_topic",rear_topic);
        nh.getParam("control_topic",control_topic);
        nh.getParam("/decision_node/limit_speed_topic",limit_speed_topic);
        nh.getParam("enable_move",enable_move_);
        odom_sub_ = nh.subscribe(odom_topic,5,&Dwa::odom_callback,this);
        front_wheel_sub_ = nh.subscribe(front_topic,5,&Dwa::front_callback,this);
        rear_wheel_sub_ = nh.subscribe(rear_topic,5,&Dwa::rear_callback,this);
        limit_speed_sub_ = nh.subscribe(limit_speed_topic,5,&Dwa::limit_speed_callback,this);
        control_pub_ = nh.advertise<ackermann_msgs::AckermannDriveStamped>(control_topic,5);
        dwa_param_init();
        need_plan = true;
        current_max_speed_ = max_speed_;
    }

    void set_status(bool plan)
    {
        need_plan = plan;
        std::cout<<"set status:"<<int(plan)<<std::endl;
    }

    void set_plan(nav_msgs::Path& path)
    {
        global_path = path;
        if(path.poses.size() == 0)
            need_plan = false;
        else
            need_plan = true;
    }


private:
    void odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
    {
        if(need_plan && global_path.poses.size()>0)
        {
            auto pose = msg->pose.pose;
            auto twist = msg->twist.twist;
            sample_control_value(pose);
        }
        else
        {
            ControlValue zero;
            pub_control_msg(zero);
        }
    }

    void limit_speed_callback(const std_msgs::Int8ConstPtr& msg)
    {
        if(msg->data == 0)
        {
            set_maxspeed(0);
            ROS_INFO("set max speed 0");
        }
        else if(msg->data == 1)
        {
            set_maxspeed(max_limit_speed_);
            ROS_INFO("set max speed limit");
        }
        else if(msg->data == 2)
        {
            ROS_INFO("set max speed unlimit");
            set_maxspeed(max_speed_);
        }
    }

    void front_callback(const control_msgs::JointControllerStateConstPtr& msg)
    {
        fb.angle = msg->process_value;
    } 

    void rear_callback(const control_msgs::JointControllerStateConstPtr& msg)
    {
        fb.speed = msg->process_value*0.05;
    }

    void pub_control_msg(ControlValue& best_control)
    {
        ackermann_msgs::AckermannDriveStamped msg;
        msg.drive.speed = best_control.speed;
        msg.drive.steering_angle = best_control.angle;
        control_pub_.publish(msg);
    }

    void sample_control_value(geometry_msgs::Pose& pose)
    {
        std::cout<<current_max_speed_<<std::endl;
        double speed_step = (max_delta_speed)*2/(speed_samples-1);
        double angle_step = (max_delta_angle)*2/(angle_samples-1);
        std::vector<ControlValue> cad_control_value;
        
        for(int i=0;i<speed_samples;i++)
        {
            for(int j=0;j<angle_samples;j++)
            {
                ControlValue control_value;
                control_value.angle = -max_delta_angle+j*angle_step+fb.angle;
                control_value.speed = -max_delta_speed+i*speed_step+fb.speed;
                if(control_value.angle<-max_angle || control_value.angle>max_angle || control_value.speed<0 || control_value.speed>current_max_speed_)
                    continue;
                cad_control_value.emplace_back(control_value);
            }
        }
        if(cad_control_value.size() == 0)
        {
            speed_step = current_max_speed_/(speed_samples-1);
            angle_step = max_angle*2/(angle_samples-1);
            for(int i=0;i<speed_samples;i++)
            {
                for(int j=0;j<angle_samples;j++)
                {
                    ControlValue control_value;
                    control_value.angle = -max_delta_angle+j*angle_step;
                    control_value.speed = i*speed_step;
                    cad_control_value.emplace_back(control_value);
                }
            }
        }
        choose_trajectory(pose,cad_control_value);
    }

    void set_maxspeed(double max_speed)
    {
        current_max_speed_ = max_speed; 
    }


    double judge_trajectory(geometry_msgs::Pose& pose,ControlValue& control_val)
    {
        auto next_pose = motion_simulation(pose,control_val);
        double distance = cal_distance_ptr_->get_min_distance(next_pose);
        double path_distance = calculate_distance_to_path(next_pose);
        auto move_distance = sqrt(pow(next_pose(0,3)-pose.position.x,2)+pow(next_pose(1,3)-pose.position.y,2));
        double score = 0;
        // std::cout<<"control_val:"<<control_val.angle<<" "<<control_val.speed;
        if(distance<10)    //距离小于10cm
        {
            // std::cout<<"   collision_weight: "<<collision_weight_*distance;
            score+=collision_weight_*distance;
        }
        else
        {
            score+=10*collision_weight_;
            // std::cout<<"collision_weight: "<<10*collision_weight_;
        }
        if(move_distance>0.1)
        {
            score+=move_weight_*move_distance;
            // std::cout<<"  move_weight:"<<move_weight_*move_distance;
        }
        else
        {
            // std::cout<<"  move_weight:"<<-10;
            score-=2;
        }
        score-=path_distance*path_weight_;
        // std::cout<<"   path weight:"<<-path_distance*path_weight_<<std::endl;
        return score;
    }

    void choose_trajectory(geometry_msgs::Pose& pose,std::vector<ControlValue>& control_value)
    {
        ControlValue best_control;
        double best_score = -DBL_MAX;
        for(int i=0;i<control_value.size();i++)
        {
            double score = judge_trajectory(pose,control_value[i]);
            if(best_score < score)
            {
                best_score = score;
                best_control = control_value[i];
            }
        }
        if(enable_move_)
        {
            std::cout<<"best_result:"<<best_control.angle<<" "<<best_control.speed<<std::endl;
            pub_control_msg(best_control);
        }
    }

    Eigen::Matrix4d motion_simulation(geometry_msgs::Pose& pose,ControlValue& control)
    {
        Eigen::Matrix4d transform_mat;
        transform::Pose2Mat4d(transform_mat,pose);
        Eigen::Vector4d(control.speed,0,0,0);
        auto spd_in_world = transform_mat*control.speed;
        auto w = tan(control.angle)/0.32*control.speed;
        double x = transform_mat(0,3);
        double y = transform_mat(1,3);
        int step = simulation_time/0.05;    //模拟次数
        Eigen::Matrix4d simulation_mat = Eigen::Matrix4d::Identity();
        simulation_mat(0,3) = control.speed*0.05;
        simulation_mat.block<3,3>(0,0) = Eigen::AngleAxisd(w*0.05, Eigen::Vector3d ( 0,0,1 )).matrix();
        for(int i=0;i<step;i++)
        {
            transform_mat*=simulation_mat;
        }
        return transform_mat;
    }

    double calculate_distance_to_path(Eigen::Matrix4d& pose_mat)
    {
        double last_distance = std::sqrt(pow(pose_mat(0,3)-global_path.poses[0].pose.position.x,2)+pow(pose_mat(1,3)-global_path.poses[0].pose.position.y,2));
        for(int i=1;i<global_path.poses.size();i++)
        {
            if(abs(pose_mat(0,3)-global_path.poses[i].pose.position.x)>1.0 || abs(pose_mat(1,3)-global_path.poses[i].pose.position.y)>1.0)
                continue;
            double distance = std::sqrt(pow(pose_mat(0,3)-global_path.poses[i].pose.position.x,2)+pow(pose_mat(1,3)-global_path.poses[i].pose.position.y,2));
            if(last_distance<distance)
            {
                return last_distance;
            }
            else
                last_distance = distance;
        }
    }

void dwa_param_init()
{
    nh.getParam("dwa_param/max_speed",max_speed_);
    nh.getParam("dwa_param/max_limit_speed",max_limit_speed_);
    nh.getParam("dwa_param/max_angle",max_angle);
    nh.getParam("dwa_param/max_delta_speed",max_delta_speed);
    nh.getParam("dwa_param/max_delta_angle",max_delta_angle);
    nh.getParam("dwa_param/speed_samples",speed_samples);
    nh.getParam("dwa_param/angle_samples",angle_samples);
    nh.getParam("dwa_param/simulation_time",simulation_time);
    nh.getParam("dwa_param/collision_weight",collision_weight_);
    nh.getParam("dwa_param/move_weight",move_weight_);
    nh.getParam("dwa_param/path_weight",path_weight_);
}


private:
    ros::NodeHandle nh;
    ros::Subscriber odom_sub_;
    ros::Subscriber front_wheel_sub_;
    ros::Subscriber rear_wheel_sub_;
    ros::Subscriber limit_speed_sub_;
    ros::Publisher control_pub_;
    std::shared_ptr<CalDistance> cal_distance_ptr_;
    ControlValue fb;

    nav_msgs::Path global_path;

    bool enable_move_;
    bool need_plan;
    double current_max_speed_;
    double max_speed_;
    double max_limit_speed_;
    double max_angle;
    double max_delta_speed;
    double max_delta_angle;
    double speed_samples;
    double angle_samples;
    double simulation_time;
    double collision_weight_;
    double move_weight_;
    double path_weight_;
};








#endif